classdef RotatingFrequencyTracker < matlab.System
    % RotatingFrequencyTracker 旋转频率跟踪器
    % 该类用于跟踪旋转信号的频率变化 参考文档 2.1. 旋转频率跟踪环路


    properties (Nontunable)

        LoopFiler 
        T_coh              % 以Tcoh为周期的积分-清除时间
        T_roll             % 以Troll为周期的积分-清除时间
        N_roll             % 对旋转频率完成一次鉴别的帧长度内的采样点数
        rho                % IIR 平滑因子

        antennaNum         % 天线数量
        pFreqDiscriminator % 频率鉴别器
        LoopFilter         % 环路滤波器

        rotAmpToZero       % 是否将旋转幅度调整为0

    end


    properties (Access = private)
        rotFreqEst % 旋转频率估计值

        xLast % 上一次的IIR输出

        genSignal % 本地旋转频率信号生成函数

        integBuf % 积分结果滑窗积累
        integBufTime % 积分结果滑窗时间 
        validIntegBuf % 积分结果滑窗是否有效

        nonUseFft % 

        tForRotFreqEst % 旋转频率估计时间点
        firstTimeFlag % 是否为第一次运行
    end


    methods (Access = public)
        function obj = RotatingFrequencyTracker(config, rxConfig)
            obj.LoopFiler = HelperLoopFilter(rxConfig.tracker.rotationFreq.noiseBandwidth,...
                rxConfig.tracker.rotationFreq.order, rxConfig.tracker.rotationFreq.flagBoxCarInteg);
            obj.T_coh = rxConfig.tracker.rotationFreq.T_coh;
            obj.T_roll = rxConfig.tracker.rotationFreq.T_roll; % 对旋转频率完成一次鉴别的帧长度
            obj.N_roll = obj.T_roll / obj.T_coh; % 对旋转频率完成一次鉴别的帧长度内的采样点数, 由于鉴别器用的是一半的时间，因此这里 是 obj.T_roll / obj.T_coh
            obj.antennaNum = config.antennas.count;
            obj.rho = rxConfig.tracker.rotationFreq.rho;
            obj.rotFreqEst = rxConfig.tracker.rotationFreq.InitialFreqOffset;
            obj.rotAmpToZero = config.rotation.rotAmpToZero;

            % 本地旋转频率信号生成 10 * antennaNums
            obj.genSignal = @(t, f_R, antennaNum) 2*pi*f_R.*t + 2*pi*(1:antennaNum)/antennaNum;
            % 频率鉴别器
            obj.pFreqDiscriminator = HelperFreqDiscriminator('atan2', false, false);
            % 环路滤波器
            obj.LoopFilter = ...
                HelperLoopFilter(rxConfig.tracker.rotationFreq.noiseBandwidth, ...
                rxConfig.tracker.rotationFreq.order, rxConfig.tracker.rotationFreq.flagBoxCarInteg);
            % 积分结果滑窗积累
            obj.integBuf = zeros(obj.antennaNum, obj.N_roll, 'like', 0+0i); 
            obj.integBufTime = zeros(1, obj.N_roll); 
            obj.validIntegBuf = 0;
            obj.tForRotFreqEst = zeros(round(rxConfig.tracker.totalIntegTime / obj.T_coh), 1);
            obj.firstTimeFlag = true;
        end

        function rotFreqEst = getFreqEst(obj)
            rotFreqEst = obj.rotFreqEst;
        end

        function checkTcoh(obj, f_R, T_B, A, N0)

            if f_R == 0 
                return;
            end

            % 检查Tcoh是否满足条件
            if obj.T_coh > 1/(12*f_R)
                error('T_coh 必须远远小于 1/12f_R');
            end
            % 2. T_coh >= T_B, 其中 T_B 为符号周期
            if obj.T_coh > 1 / (10 *  T_B )
                error('T_coh 必须远远小于 T_B');
            end

            % 3. 保证积分清除结果中的信噪比大于 0 dB, 这一部分后面再修改? fixme
            % if A^2 * obj.T_coh / N0 > 0
            %     error('积分清除结果中的信噪比必须大于 0 dB');
            % end
        end

        function checkTroll(obj, f_R)

            if f_R == 0 
                return;
            end
            
            % 检查Troll是否满足条件
            % obj.N_roll = obj.T_roll / obj.T_coh;
            if obj.T_roll < 10 / f_R
                error('T_roll  必须远远大于 1 / f_R');
            end

            % 鉴别器的要求: 3. 三倍的频率测量均方差不得超过四分之一的频率牵入范围， 这一部分后面再补充 fixme
        end

    end

    methods (Access = protected)
        function setupImpl(obj)
            % 初始化上一次的IIR输出为 0
            obj.xLast = 0;
        end

        function [integRes, fqyerr] = stepImpl(obj, inputSignal, t)
            % 跟踪旋转频率
            % 输入信号: 所有天线上经载波剥离和码剥离后准备进行T_coh 积分-清除的信号
            % 输出信号: 经过T_coh 积分-清除的信号/估计的旋转频率

            % 具体实现思路, see: ref/自适应时域波束成形，主要步骤如下:

            % -1. 时间
            if obj.firstTimeFlag
                obj.tForRotFreqEst = t + obj.tForRotFreqEst; 
                obj.firstTimeFlag = false;
            end

            % 0. 先验
            rotFreqEst = obj.rotFreqEst;

            % 1. 积分-清除
            % 积分清楚时间需要保证远小于旋转周期 \(1/f_R\)、符号周期 \(T_B\)，而且需要保证信噪比> 0 dB)
            % 检查输入的u是否是  ?  * ? * 天线数量
            if size(inputSignal, 3) ~= obj.antennaNum 
                error('输入信号维度错误');
            end
            % obj.antennaNum = size(u, 3);

            % if antennaNum != 1:  3819 10 3-> 1 10 3 -> 10 *  3
            % if antennaNum == 1:  3819 10  -> 1 10  -> 1 * 10
            integRes = squeeze(sum(inputSignal, 1));

            if obj.antennaNum ~= 1
                integRes = integRes.'; % 天线数量 * len 的向量
            end

            % 第一次积分结果滑窗积累
            len = size(integRes, 2);     % 第一次积分结果的长度
            obj.integBuf = [obj.integBuf(:, (len+1):end), integRes]; % 新的积分结果放在后面 
            obj.integBuf = obj.integBuf(:, 1:obj.N_roll);  
            obj.integBufTime = [obj.integBufTime(:, (len+1):end), obj.tForRotFreqEst.']; 
            obj.integBufTime = obj.integBufTime(1:obj.N_roll);  
            obj.tForRotFreqEst = obj.T_coh * len + obj.tForRotFreqEst;
            obj.validIntegBuf = min(obj.validIntegBuf + len, obj.N_roll);

            if obj.validIntegBuf >= obj.N_roll
                % 非滑动窗口 or 滑动窗口
                obj.validIntegBuf = obj.validIntegBuf - obj.N_roll;

                % 2. 取绝对值    
                r_m = abs(obj.integBuf); 

                % 3. 直流分量去除（提取旋转相关的时变分量）
                % 通过**IIR 平滑滤波**对多帧、多天线的信号幅度取平均，估计直流分量 
                % 去除直流分量后信号仅保留与旋转相关的时变分量
                x = sum(sum(r_m, 1))  / (obj.antennaNum * obj.N_roll);

                % IIR 平滑滤波,可能存在问题
                uDC = obj.rho * x + (1 - obj.rho) * obj.xLast;
                obj.xLast = x; 

                uNonDC = r_m - uDC; % 不同帧下的直流信号是不同的;

                obj.nonUseFft = true;
                if obj.nonUseFft
                    % 4. 与正交本地旋转频率信号进行混频(提取旋转频率偏差)
                    % iMix = uNonDC .* cos( - obj.genSignal(obj.integBufTime.', rotFreqEst, obj.antennaNum)).';
                    % qMix = uNonDC .* sin( - obj.genSignal(obj.integBufTime.', rotFreqEst, obj.antennaNum)).';
                    carrier = exp(-1j * obj.genSignal(obj.integBufTime.', rotFreqEst, obj.antennaNum));
                    % 5. 对混频结果再次作积分清除, 之后再对各个天线结果求和。这里直接对各个天线求和。混频结果的积分清除实际在鉴频器中完成。
                    % curIntegRes = sum(iMix, 1) + 1i * sum(qMix, 1); % 多天线求和的积分结果 1 * len 的向量
                    curIntegRes = sum(uNonDC .* carrier.', 1); % 多天线求和的积分结果 1 * len 的向量
                

                    % 6. 鉴频器
                    fqyerr = obj.pFreqDiscriminator(curIntegRes, obj.T_roll);  
                    fqyerrDelta = obj.LoopFilter.step(fqyerr, obj.T_roll);
                    obj.rotFreqEst = fqyerrDelta + rotFreqEst;
                else
                    fqyerr = 0;
                    obj.rotFreqEst =  getMaxBaseFreq(r_m(1, :), 1 / obj.T_coh);
                end
            else
                fqyerr =  10 / obj.T_roll;
            end
        end


        function resetImpl(obj)
            % obj.pFreqDiscriminator.reset();
            obj.LoopFilter.reset();
            obj.integBuf = zeros(obj.antennaNum, obj.N_roll, 'like', 100+100i); 
            obj.integBufTime = -1 * ones(1, obj.N_roll); 
            obj.validIntegBuf = 0;
        end
    end



end